/*
 * TelemetryFrame.cpp
 *
 *  Created on: 10.12.2012
 *      Author: Michael
 */

#include "TelemetryFrame.h"
#include "stdint.h"

#include "protocol.h"

TelemetryFrame::TelemetryFrame(int msgType) {
	this->buffer[0] = FRAMESTART1;
	this->buffer[1] = FRAMESTART2;
	this->buffer[2] = FRAMESTART3;
	this->buffer[3] = FRAMESTART4;

	this->buffer[4] = 0;
	this->buffer[5] = 0;

	this->buffer[6] = msgType;
	this->length = 7;
}

TelemetryFrame::~TelemetryFrame() {
	// TODO Auto-generated destructor stub
}

void TelemetryFrame::addValue(float value){
	if(this->length == 7) {
		this->buffer[this->length] = HEADSEPERATOR;
	} else {
		this->buffer[this->length] = VALSEPERATOR;
	}

	char floatbuF[sizeof(float)];
	Float2Char(floatbuF, value);

	for (uint16_t i = 1; i <= sizeof(float); i++) {
		this->buffer[this->length + i] = floatbuF[i - 1];
	}

	this->length += (sizeof(float) + 1);
}

void TelemetryFrame::endFrame(){
	this->buffer[this->length] = FRAMEEND;
	this->length++;

	uint8_t low = length & 0xFF;
	uint8_t high = (length & 0xFF00) >> 8;

	this->buffer[4] = high;
	this->buffer[5] = low;

}

void TelemetryFrame::Float2Char(char* buf, float in) {
	uint32_t* raw_data = (uint32_t*) ((void*) (&in));
	for (uint16_t i = 1; i <= sizeof(float); i++) {
		buf[sizeof(float) - i] = *raw_data & 0xFF;
		*raw_data = *raw_data >> 8;
	}
}
